//遥控船测试 2022——11——03
#include <esp_now.h>
#include <WiFi.h>  //espnow
#include <Wire.h>
#include "SSD1306Wire.h"  //oled
#include <Arduino.h>      //moter
#include <Servo.h>        //Servo
#include <JY901.h>      //姿态传感器
Servo servo;
//电机引脚
#define moter1 26
#define moter2 25
#define moter3 33
#define moter4 32


//IIC——oled引脚            21  22
SSD1306Wire display(0x3c, SDA, SCL);

static const int servoPin = 15;  //舵机引脚

//超声波
const int TrigPin1 = 4;   //Trig
const int EchoPin1 = 2;   //Echo  ---后端
const int TrigPin2 = 13;  //Trig
const int EchoPin2 = 12;  //Echo ---前端
float distance1;          //超声波距离变量
float distance2;

int counter = 1;   //oled计数
int freq = 2000;   //设置PWM波的频率
int channel1 = 4;  //设置通道,共16个通道，0~15
int channel2 = 5;
int channel3 = 2;
int channel4 = 3;

int resolution = 10;  //分辨率，取值0~20 duty的最大值为 2^resolution-1
bool val = 0;

Servo servo6;  //舵机
int SOar = 0;  //舵机角度

int Angle_now = 0;//船身此时刻角度
int Angle_auto = 0;//在开启auto模式时记录船身的角度
//PID参数初始化
float time1, duration1, distance_pid;
//PID constants****************************************************************
float setPoint = 0;    //滑轨中心与测距模块的距离
float error;           //当前误差
float errorA; 
float errorD; 
float previous_errorD;  //上一时刻误差，用来计算D
float kp = 5;          //10
float ki = 0;          //0.05
float kd = 0;          //200
int dt = 50;           //每50毫秒进行一次计算
int P, I, D, PID;
int speedL = 0;  //左电机的速度  开闭环共用接口，方便oled监测，后期出事了再改
int speedR = 0;  //右电机的速度
int pid_key = 0;
//*****************************************************************************
//esp——now 创建一个结构体接收数据
// 接收端的MAC地址
uint8_t broadcastAddress[] = { 0xC8, 0xF0, 0x9E, 0xAC, 0xA5, 0x04 };  //C8:F0:9E:AC:A5:04

// 创建一个结构体接收数据
typedef struct struct_message {
  int rx;
  int ry;
  int lx;
  int ly;
  int lok;
  int rok;
  int AUTOjudg;  //自控开关
  //
  int Angle; 
  int sp1;
  int sp2;
  int ml;
  int mr;
  int so;
  int led;
} struct_message;
int connectedjudg = 1;
// 创建一个结构体变量
struct_message myData;

// 回调函数,当收到消息时会调佣该函数
void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) {
  memcpy(&myData, incomingData, sizeof(myData));
  Serial.print("Bytes received: ");
  Serial.println(len);
  Serial.print("a: ");
  Serial.println(myData.rx);
  Serial.print("b: ");
  Serial.println(myData.lx);
  Serial.print("c: ");
  Serial.println(myData.ly);
  Serial.print("d: ");
  Serial.println(myData.ry);
  Serial.println();
}
//ESPnow 发送函数
void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status) {
  Serial.print("\r\nLast Packet Send Status:\t");
  Serial.println(status == ESP_NOW_SEND_SUCCESS ? "Delivery Success" : "Delivery Fail");
  if (status == ESP_NOW_SEND_SUCCESS) {
    connectedjudg = 1;
  } else {
    connectedjudg = 0;
  }
}
09==
void setup() {
  Serial.begin(115200);
   JY901.StartIIC();//姿态初始化
  //esp——now初始化
  // 设置wifi模式
  WiFi.mode(WIFI_STA);
  // 初始化esp-now
  if (esp_now_init() != ESP_OK) {
    Serial.println("Error initializing ESP-NOW");
    return;
  }
  //注册接收信息的回调函数
  esp_now_register_recv_cb(OnDataRecv);
  esp_now_register_send_cb(OnDataSent);
  // 注册通信频道
  esp_now_peer_info_t peerInfo;
  memcpy(peerInfo.peer_addr, broadcastAddress, 6);
  peerInfo.channel = 0;      //通道
  peerInfo.encrypt = false;  //是否加密为False
  if (esp_now_add_peer(&peerInfo) != ESP_OK) {
    Serial.println("Failed to add peer");
    return;
  }
  //oled初始化
  display.init();
  display.flipScreenVertically();
  display.setFont(ArialMT_Plain_10);
  //左轮
  ledcSetup(channel1, freq, resolution);  //设置通道0
  ledcAttachPin(moter1, channel1);        //将通道0和gpio_pin连接起来
  ledcSetup(channel2, freq, resolution);  //设置通道1
  ledcAttachPin(moter2, channel2);
  //右轮
  ledcSetup(channel3, freq, resolution);  //设置通道3
  ledcAttachPin(moter3, channel3);
  ledcSetup(channel4, freq, resolution);  //设置通道4
  ledcAttachPin(moter4, channel4);
  //舵机
  servo6.attach(servoPin);

  //超声波
  pinMode(TrigPin1, OUTPUT);
  pinMode(EchoPin1, INPUT);
  pinMode(TrigPin2, OUTPUT);
  pinMode(EchoPin2, INPUT);
}


//电机控制函数
// 电机的控制程序，分别是左右两个轮子的占空比（0-1024）
void Motor_Control(int Cnt_L, int Cnt_R) {
  if (Cnt_L >= 0)  // 左轮正向转
  {
    ledcWrite(channel1, Cnt_L);
    ledcWrite(channel2, 0);
  } else  // 左轮反向转
  {
    ledcWrite(channel1, 0);
    ledcWrite(channel2, -Cnt_L);
  }

  if (Cnt_R >= 0)  // 右轮正向转
  {
    ledcWrite(channel3, Cnt_R);
    ledcWrite(channel4, 0);
  } else  // 右轮反向转
  {
    ledcWrite(channel3, 0);
    ledcWrite(channel4, -Cnt_R);
  }
}

//测距函数
void superget(void) {
  digitalWrite(TrigPin1, LOW);
  delayMicroseconds(2);
  digitalWrite(TrigPin1, HIGH);
  delayMicroseconds(10);
  digitalWrite(TrigPin1, LOW);
  // 检测脉冲宽度，并计算出距离
  distance1 = pulseIn(EchoPin1, HIGH) / 58.00;

  digitalWrite(TrigPin2, LOW);
  delayMicroseconds(2);
  digitalWrite(TrigPin2, HIGH);
  delayMicroseconds(10);
  digitalWrite(TrigPin2, LOW);
  // 检测脉冲宽度，并计算出距离
  distance2 = pulseIn(EchoPin2, HIGH) / 58.00;


  /* Serial.print(distance1);//串口打印
Serial.print("cm");
Serial.println();
Serial.print(distance2);//串口打印
Serial.print("cm");
Serial.println(); */
  //delay(1000);//延时防止撑爆串口
}

void superget_OLED(void) {
  display.clear();
  display.setFont(ArialMT_Plain_10);
  display.drawString(70, 5, "RX:" + String(myData.rx));
  display.drawString(20, 5, "LX:" + String(myData.lx));
  display.drawString(70, 15, "RY:" + String(myData.ry));
  display.drawString(20, 15, "LY:" + String(myData.ly));
  display.drawString(20, 30, "VL:" + String(speedL));
  display.drawString(70, 30, "VR:" + String(speedR));
  display.drawString(70, 40, "SO:" + String(SOar));
  display.drawString(20, 50, String(distance1) + "CM");
  display.drawString(70, 50, String(distance2) + "CM");
  display.display();
}

void JY901get(void) {
  JY901.GetAngle();//获取角度
   // if ((float)JY901.stcAngle.Angle[2]/32768*180>=0)
    Angle_now = (float)JY901.stcAngle.Angle[2]/32768*180 ;//获得基准角度  
    //else Angle_auto = (float)JY901.stcAngle.Angle[2]/32768*180 + 360 ;//获得基准角度  
  // Serial.print("Angle0:");Serial.print(" ");Serial.println(Angle901);
  // Serial.print("Angle1:");Serial.print(" ");Serial.println((float)JY901.stcAngle.Angle[2]/32768*180);
}

void Speed_Set(void) {
  /* speedL speedR */
  //直线速度
  if (myData.ly > 4000)  //限速
  { myData.ly = 4000; }
  if (myData.ly < 2200 && myData.ly > 1800)  //防死区
  {
    speedL = 0;
    speedR = 0;
  } else {
    if (myData.ly <= 1800) {
      speedL = -(4000 - myData.ly) / 4;  //550-1000
      speedR = -(4000 - myData.ly) / 4;
    }
    if (myData.ly >= 2200) {
      speedL = myData.ly / 4;  //550-1000
      speedR = myData.ly / 4;
    }
  }
  //转向
  if (myData.lx > 4000) { myData.lx = 4000; }
  if (myData.lx > 2200 || myData.lx < 1800) {
    if (myData.lx <= 1800) {
      speedL += (4000 - myData.lx) / 20;  //差速290-500
      speedR -= (4000 - myData.lx) / 20;
    }
    if (myData.lx >= 2200) {
      speedL -= myData.lx / 20;  //差速290-500
      speedR += myData.lx / 20;
    }
  }
  if (speedL > -270 && speedL < 270)  //防死区
  { speedL = 0; }
  if (speedR > -270 && speedR < 270) { speedR = 0; }
  if (speedL > 1020)  //限幅
  { speedL = 1020; }
  if (speedR > 1020) { speedR = 1020; }
}
void Servo_Control(void) {
  if (myData.ry > 3000) {
    SOar = 180;
    servo6.write(SOar);
  }
  if (myData.ry < 1000) {
    SOar = 0;
    servo6.write(SOar);
  }
}
void ESPnow_sent(void) {
  myData.sp1 = distance1;  //测距1
  myData.sp2 = distance2;  //测距2
  myData.ml = speedL;
  myData.mr = speedR;
  myData.so = SOar;
  myData.led = PID;  //灯
  myData.Angle = Angle_now;
  esp_err_t result = esp_now_send(broadcastAddress, (uint8_t *)&myData, sizeof(myData));
}



void PID_OLED(void) {
  display.clear();
  display.setFont(ArialMT_Plain_10);
  display.drawString(20, 5, "VL:" + String(speedL));
  display.drawString(70, 5, "VR:" + String(speedR));
  //display.drawString(70, 40, "SO:" + String(SOar));
  display.drawString(20, 20, "^ " + String(distance2) + "CM");
  display.drawString(20, 35, "v " + String(distance1) + "CM");
  //pid参数
  display.drawString(80, 20, "KP:" + String(P));
  display.drawString(80, 35, "kI:" + String(I));
  display.drawString(80, 50, "ER:" + String(error));
  display.drawString(20, 50, "MODE:" + String(pid_key));
  display.display();
}



void AUTO_Control(void) {
    if(myData.AUTOjudg == 1){
    JY901.GetAngle();//获取角度 
   // if ((float)JY901.stcAngle.Angle[2]/32768*180>=0)
    Angle_auto = (float)JY901.stcAngle.Angle[2]/32768*180 ;//获得基准角度  
   // else Angle_auto = (float)JY901.stcAngle.Angle[2]/32768*180 + 360 ;//获得基准角度  
    }
  while (myData.AUTOjudg == 1) {
    /////////////////////////////////////////////////////////////////////
    
    superget();  //超声波获取距离
    JY901.GetAngle();//获取角度 
    
    Angle_now = (float)JY901.stcAngle.Angle[2]/32768*180;//获得实时角度  
    
    speedL = 600;
    speedR = 640;
    // PID Calculation, PID计算————1：分时运算*************************************************
    /*  if(pid_key == 0)*/
/*
    error = distance1 - distance2;      //计算误差
    if(error<1&&error>-1) error=0;      //粗心忽略小值   
    P = 7.5 * (error - previous_error);  //P项
    if (-20 < error && error < 20) {
      I += ki * error;
    }  //I项
    else {
      I = 0;
    }
    D = kd * ((error - previous_error) / dt);  //D项，
    PID += P + I + D;                           //PID
    if (PID > 100) { PID = 100; }              //限幅
    if (PID < -100) { PID = -100; }            //限幅

    previous_error = error;  //更新上一时刻误差
  */
 
    ///////////////////////////
    // PID Calculation, PID计算————2：重视度运算*************************************************
     
    
    errorA = -Angle_auto + Angle_now ; //计算角度误差
    P = 20 * errorA;  //P项
    PID = P ;                           //PID
    if (PID > 100) { PID = 100; }              //限幅
    if (PID < -100) { PID = -100; }            //限幅
     
    //previous_errorA = errorA;  //更新上一时刻误差
     
     if(errorA<=3&&errorA>=-3){
    errorD = distance1 - 166;      //计算误差
    // if(errorD<1&&errorD>-1) errorD=0;      //粗心忽略小值   
     P = 4 * errorD  ;  //P项
     D = 15 * ((errorD - previous_errorD) / dt);
     PID = -P ;                           //PID
     if (PID > 100) { PID = 100; }              //限幅
     if (PID < -100) { PID = -100; }            //限幅
     previous_errorD = errorD;  //更新上一时刻误差
     }
 // PID Calculation, PID计算————3：姿态控量运算*************************************************
     //
     //ngle901
     
    /* error =-( Angle_auto - Angle_now );      //计算误差,基准减去实时角度
    //if(error<1&&error>-1) error=0;      //粗心忽略小值   
    P = 40 * error;  //P项
    if (-20 < error && error < 20) {
      I += ki * error;
    }  //I项
    else {
      I = 0;
    }
    D = kd * ((error - previous_error) / dt);  //D项，
    PID = P + I + D;                           //PID
    if (PID > 100) { PID = 100; }              //限幅
    if (PID < -100) { PID = -100; }            //限幅

    previous_error = error;  //更新上一时刻误差
 */
    // Servo Control，用计算结果控制舵机*******************************************
    // if (PID > 0) {      //双电机增速控制
      speedL += 1.2*PID;
      //speedR = 630;
    // }
    // if (PID < 0) {
    //   speedL = 600;
    //   speedR -= PID;
    // }

    //PID_OLED();     //oled
    ESPnow_sent();  //回传信息至遥控器
    Motor_Control(speedL, speedR);
    delay(50);
    if (pid_key == 0) pid_key = 1;  //模式切换
    else pid_key = 0;               //模式切换
    ////////////////////////////////////////////////////////////////////
    if (myData.AUTOjudg == 0) break;
  }
}

void Lose_Control(void) {
  while (connectedjudg == 0) {
    ESPnow_sent();        //回传信息至遥控器
    Motor_Control(0, 0);  //电机休止
    display.clear();
    display.setFont(ArialMT_Plain_24);  //字体大小
    display.drawString(2, 15, "Un Connected");
    display.display();
    if (connectedjudg == 1) break;
  }
}


void loop() {
  
  AUTO_Control();                 //自控
  JY901get();                     //获取角度
  superget();                     //超声波获取距离
  superget_OLED();                //oled
  Servo_Control();                //舵机
  Speed_Set();                    //PID
  Motor_Control(speedL, speedR);  //电机输出
  ESPnow_sent();                  //回传信息至遥控器
  Lose_Control();                 //失控判断
}